//
// Created by cyq on 2022/12/5.
//
/**
  ******************************************************************************
  * @file    driver_motor.c
  * @author  XJTU ROBOMASTER Hero Team
  * @brief   motor driver.
  *          This file provides functions to process motor data and update data:
  *           + Initialization functions
  *           + Data update functions
  *
  @verbatim
  ==============================================================================
                        ##### How to use this driver #####
  ==============================================================================
    [..]
      (#) Initialize the motor by implementing the MotorInit():
			(++) Initialize the motor parameters, including:
			     (+++) pid parameters

      (#) Update motor data by implementing the MotorDataUpdate():
         (++) Update motor parameters, including:
						 (+++) ReceiveMessege
						 (+++) Current
						 (+++) Torque
						 (+++) EncoderData

  @endverbatim
  ******************************************************************************
  * @attention
  *
  * Please add comments after adding or deleting functions to ensure code
	* specification.
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "driver_motor.h"
#include "driver_chassis.h"
#include "ramp.h"

MotorStruct ChassisMotor[5];
DM_MotorStruct yawMotor = {0};
float Mean_Fliter_K = 0.95;//均值滤波系数
int float_to_uint(float x, float x_min, float x_max, int bits)
{
    /// Converts a float to an unsigned int, given range and number of bits ///
    float span = x_max - x_min;
    float offset = x_min;
    return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    /// converts unsigned int to float, given range and number of bits ///
    float span = x_max - x_min;
    float offset = x_min;
    return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
}
void MotorDataUpdate(MotorStruct* motor)
{
    uint8_t i;
    int16_t SpeedSum = 0;

    motor->EncoderDataLast = motor->EncoderData;
    motor->EncoderData = (int16_t)((motor->ReceiveMessege[0] << 8) | motor->ReceiveMessege[1]);

    if (motor->EncoderData - motor->EncoderDataLast > 6400)
    {
        motor->EncoderCount--;
    }

    if (motor->EncoderData - motor->EncoderDataLast < -6400)
    {
        motor->EncoderCount++;
    }
    motor->RawSpeed = (int16_t)((motor->ReceiveMessege[2] << 8) | motor->ReceiveMessege[3]);
    motor->RawCurrent = (int16_t)((motor->ReceiveMessege[4] << 8) | motor->ReceiveMessege[5]);
    motor->RawTorque = (int16_t)((motor->ReceiveMessege[4] << 8) | motor->ReceiveMessege[5]);
    motor->RawTemperature = (int8_t)motor->ReceiveMessege[6];
    motor->Speed_array[0] = motor->RawSpeed;
    SpeedSum += motor->Speed_array[0];
    for (i = 0; i < 9; i++)
    {
        motor->Speed_array[i + 1] = motor->Speed_array[i];
        SpeedSum += motor->Speed_array[i + 1];
    }
    motor->MeanFilterSpeed = SpeedSum / 10;
    motor->LowPassFilterSpeed = motor->Speed_array[0] * Mean_Fliter_K + motor->Speed_array[1] * (1 - Mean_Fliter_K);
    motor->Location.Location = motor->EncoderCount + (float)motor->EncoderData / 8191.0f;

    if (motor->Location.Location > 9000.0f)
    {
        motor->Location.Location -= 9000.0f;
        motor->EncoderCount -= 9000;
        motor->Location.SetLocation -= 9000.0f;
    }

    if (motor->Location.Location < -6000.0f)
    {
        motor->Location.Location += 6000.0f;
        motor->EncoderCount += 6000;
        motor->Location.SetLocation += 6000.0f;
    }

    motor->Speed.Speed = motor->RawSpeed / 9600.0f;
}
uint8_t Motor4310FeedbackFrameDataUpdate(DM_MotorStruct* motor)
{
    int pos_tmp, vel_tmp, tor_tmp;
    uint8_t i;
    float SpeedSum;
    /* 解算电机反馈帧的数据 */
    motor->RawFeedback.CAN_ID = (motor->ReceiveMessege[0]) & 0x0F;
    motor->RawFeedback.State = (motor->ReceiveMessege[0]) >> 4;
    pos_tmp = (motor->ReceiveMessege[1] << 8) | motor->ReceiveMessege[2];
    vel_tmp = (motor->ReceiveMessege[3] << 4) | (motor->ReceiveMessege[4] >> 4);
    tor_tmp = ((motor->ReceiveMessege[4] & 0xF) << 8) | motor->ReceiveMessege[5];
    motor->RawFeedback.Position = uint_to_float(pos_tmp, P_MIN, P_MAX, 16);
    motor->RawFeedback.Velocity = uint_to_float(vel_tmp, V_MIN, V_MAX, 12);
    motor->RawFeedback.Torque = uint_to_float(tor_tmp, T_MIN, T_MAX, 12);
    motor->RawFeedback.MosTemperature = (float)(motor->ReceiveMessege[6]);
    motor->RawFeedback.CoilTemperature = (float)(motor->ReceiveMessege[7]);

    if (motor->RawFeedback.Position - motor->LastPosition > P_MAX)
        motor->EncoderCount--;
    if (motor->RawFeedback.Position - motor->LastPosition < P_MIN)
        motor->EncoderCount++;
    motor->Location.Location =
            (float)motor->EncoderCount * (P_MAX - P_MIN) / (2 * PI) + motor->RawFeedback.Position / (2 * PI);
    motor->LastPosition = motor->RawFeedback.Position;

    motor->Speed_array[0] = motor->RawFeedback.Velocity;
    SpeedSum = motor->Speed_array[0];
    for (i = 0; i < 9; i++)
    {
        motor->Speed_array[i + 1] = motor->Speed_array[i];
        SpeedSum += motor->Speed_array[i + 1];
    }
    motor->MeanFilterSpeed = SpeedSum / 10.0f;
    motor->LowPassFilterSpeed = motor->Speed_array[0] * motor->Speed_MeanFilter_K
                                + motor->Speed_array[1] * (1 - motor->Speed_MeanFilter_K);
    motor->Speed.Speed = motor->RawFeedback.Velocity / V_MAX;

    return (motor->CAN_ID == motor->RawFeedback.CAN_ID);
}

void MotorPIDCalculate(MotorStruct *motor)
{
    switch (motor->PIDMode)
    {
        case MOTOR_PID_MODE_SPEED:
            motor->PIDSpeed.Ref = (motor->Speed.SetSpeed);
            motor->PIDSpeed.Fdb = motor->Speed.Speed;
            motor->PIDSpeed.calc(&motor->PIDSpeed);
            break;
        case MOTOR_PID_MODE_LOCATION:
            motor->PIDLocation.Ref = motor->Location.SetLocation;
            motor->PIDLocation.Fdb = motor->Location.Location;
            motor->PIDLocation.calc(&motor->PIDLocation);
            motor->PIDSpeed.Ref = motor->PIDLocation.Out;
            motor->PIDSpeed.Fdb = motor->Speed.Speed;
            motor->PIDSpeed.calc(&motor->PIDSpeed);
            break;
    }
#if !STATUS_TESTING_SET_SPEED
    if(chassisControlData.chassisMode == CHASSIS_MODE_FREE)
        motor->PIDSpeed.Out = 0.0f;
#endif
}




